import matplotlib.pyplot as plt
import matplotlib
import csv
import h5py
import numpy as np

path = '/home/ubuntu/user_space/EKF-gilbert/EKFresult.csv'
pathRaw = '/home/ubuntu/user_space/Ronin/root_dir/a028_3/data.hdf5'


f = h5py.File(pathRaw, "a")
synced = f["synced"]
gyro = synced["gyro"]
acce = synced["acce"]

acce_x, acce_y, acce_z = [], [], []


# for r in range(1, 158226) :
#     acce_x.append(acce[r][0])
#     acce_y.append(acce[r][1])
#     acce_z.append(acce[r][2])

print(acce.shape)
# print(acce_x.shape)


#### plot quaternion part
pose = f["pose"]
tango_ori = pose["tango_ori"]
ekf_ori = pose["ekf_ori"]

tango_pos = pose["tango_pos"]

tango_q0, tango_q1, tango_q2, tango_q3 = [], [], [], []
ekf_q0, ekf_q1, ekf_q2, ekf_q3 = [], [], [], []
tango_px, tango_py, tango_pz = [], [], []

for r in range(1, 20001) :
    # tango_q0.append(tango_ori[r][0])
    # tango_q1.append(tango_ori[r][1])
    # tango_q2.append(tango_ori[r][2])
    # tango_q3.append(tango_ori[r][3])
    # ekf_q0.append(ekf_ori[r][0])
    # ekf_q1.append(ekf_ori[r][1])
    # ekf_q2.append(ekf_ori[r][2])
    # ekf_q3.append(ekf_ori[r][3])
    tango_px.append(tango_pos[r][0])
    tango_py.append(tango_pos[r][1])
    tango_pz.append(tango_pos[r][2])

for r in range(1, 20001):
    tango_q0.append(tango_ori[r][0])
    tango_q1.append(tango_ori[r][1])
    tango_q2.append(tango_ori[r][2])
    tango_q3.append(tango_ori[r][3])

print("Ori loaded")


# fig, axes = plt.subplots(nrows = 3)
# time = np.arange(0, 158226, 1)
time = np.arange(0, 20000, 1)
#
# axes[0].set(title = "x axis", ylabel = 'acce', xlabel = 'time')
# axes[1].set(title = "y axis", ylabel = "acce", xlabel = "time")
# axes[2].set(title = "z axis", ylabel = "acce", xlabel = "time")
#
# axes[0].plot(time, acce_x, color = 'red')
# axes[1].plot(time, acce_y, color = 'red')
# axes[2].plot(time, acce_z, color = 'red')

# plt.show()


r = csv.reader(open(path))
racce_x, racce_y, racce_z = [], [], []

r_q0, r_q1, r_q2, r_q3 = [], [], [], []

p_x, p_y, p_z = [], [], []

next(r)
cnt = 0
for row in r :
    if cnt == 20000:
        break
    print(row[7])
    racce_x.append(float(row[7]))
    racce_y.append(float(row[8]))
    racce_z.append(float(row[9]))
    r_q0.append(float(row[0]))
    r_q1.append(float(row[1]))
    r_q2.append(float(row[2]))
    r_q3.append(float(row[3]))
    p_x.append(float(row[4]))
    p_y.append(float(row[5]))
    p_z.append(float(row[6]))
    cnt += 1


print("Data loaded")
# fig_r , axes_r = plt.subplots(nrows = 3)
# axes_r[0].set(title = "x axis", ylabel = 'acce', xlabel = 'time')
# axes_r[1].set(title = "y axis", ylabel = "acce", xlabel = "time")
# axes_r[2].set(title = "z axis", ylabel = "acce", xlabel = "time")

# axes[0].plot(time, racce_x, color = 'yellow')
# axes[1].plot(time, racce_y, color = 'yellow')
# axes[2].plot(time, racce_z, color = 'yellow')

# plt.show()

fig_q, axes_q = plt.subplots(nrows = 4)
axes_q[0].plot(time, tango_q0, color = 'red')
axes_q[1].plot(time, tango_q1, color = 'red')
axes_q[2].plot(time, tango_q2, color = 'red')
axes_q[3].plot(time, tango_q3, color = 'red')

axes_q[0].plot(time, r_q0, color = 'yellow')
axes_q[1].plot(time, r_q1, color = 'yellow')
axes_q[2].plot(time, r_q2, color = 'yellow')
axes_q[3].plot(time, r_q3, color = 'yellow')

axes_q[0].set(title = "Q", ylabel = "q0", xlabel = "time")
axes_q[1].set(title = "Q", ylabel = "q1", xlabel = "time")
axes_q[2].set(title = "Q", ylabel = "q2", xlabel = "time")
axes_q[3].set(title = "Q", ylabel = "q3", xlabel = "time")

distance_xy, distance_x, distance_y = [], [], []

for j in range(0, 5000) :
    distance_x.append(p_x[j] - tango_px[j])
    distance_y.append(p_y[j] - tango_py[j])
    distance_xy.append(abs(p_x[j] - tango_px[j]) + abs(p_y[j] - tango_py[j]))

distance_time = np.arange(0, 5000, 1)

turning_tx, turning_ty, turning_px, turning_py = [], [], [], []
for k in range(10000, 12501) :
    turning_tx.append(tango_px[k])
    turning_ty.append(tango_py[k])
    turning_px.append(p_x[k])
    turning_py.append(p_y[k])
turning_time = np.arange(10000, 12501, 1)


fig_p, axes_p = plt.subplots(nrows = 3)
axes_p[0].plot(time, tango_px, color = 'red')
axes_p[1].plot(time, tango_py, color = 'red')
axes_p[2].plot(time, tango_pz, color = 'red')

axes_p[0].plot(time, p_x, color = 'yellow')
axes_p[1].plot(time, p_y, color = 'yellow')
axes_p[2].plot(time, p_z, color = 'yellow')
axes_p[0].set(title = "tango and EKF")

fig_tango_p, axes_tango = plt.subplots(nrows = 3)
axes_tango[0].plot(time, tango_px, color = 'blue')
axes_tango[1].plot(time, tango_py, color = 'blue')
axes_tango[2].plot(time, tango_pz, color = 'blue')
axes_tango[0].set(title = "tango_pos")


plt.plot(distance_time, distance_xy, color = 'red')
plt.title('x-y distance from tango')

fig_distance, axes_dis = plt.subplots(nrows = 2)
axes_dis[0].plot(distance_time, distance_x, color = 'red')
axes_dis[1].plot(distance_time, distance_y, color = 'red')
axes_dis[0].set(title = 'x and y')


fig_turning, axes_turning = plt.subplots(nrows = 2)
axes_turning[0].plot(turning_time, turning_tx, color = 'red')
axes_turning[1].plot(turning_time, turning_ty, color = 'red')

axes_turning[0].plot(turning_time, turning_px, color = 'yellow')
axes_turning[1].plot(turning_time, turning_py, color = 'yellow')
axes_turning[0].set(title = 'turning point')



plt.show()